// mt6701_sensor.cpp
#include "MT6701CT_STD.h"

extern float encoder_angle;
extern float motor_logic_angle;
extern SPI_HandleTypeDef hspi1;
extern uint16_t rawdata;
// MT6701Sensor.cpp 顶部
MT6701Sensor MT6701Sensor::_instance = MT6701Sensor(&hspi1, MT6701_CS_GPIO_Port, MT6701_CS_Pin, 7);

// 修改 getInstance 实现
MT6701Sensor& MT6701Sensor::getInstance() {
    return _instance;
}

float cycle_diff(float diff, float cycle) {
    if (diff > (cycle / 2)) diff -= cycle;
    else if (diff < (-cycle / 2)) diff += cycle;
    return diff;
}

// Dummy CRC function for example
uint8_t calculate_crc(uint32_t data) {
    // CRC 校验算法，根据 MT6701 数据手册实现
    uint8_t crc = 0;
    for (int i = 0; i < 18; i++) {
        crc ^= (data >> i) & 0x01;
    }
    return crc & 0x3F;
}

// MT6701Sensor MT6701Sensor::getInstance() {
//     return MT6701Sensor(&hspi1, MT6701_CS_GPIO_Port, MT6701_CS_Pin, 7);
// }

MT6701Sensor::MT6701Sensor(SPI_HandleTypeDef* spi, GPIO_TypeDef* csPort, uint16_t csPin, uint8_t polePairs)
    : _spi(spi), _csPort(csPort), _csPin(csPin), _polePairs(polePairs) {}

void MT6701Sensor::csEnable() {
    HAL_GPIO_WritePin(_csPort, _csPin, GPIO_PIN_RESET);
}

void MT6701Sensor::csDisable() {
    HAL_GPIO_WritePin(_csPort, _csPin, GPIO_PIN_SET);
}

uint16_t MT6701Sensor::getRawData() {
    uint8_t txData[2] = { 0xFF, 0xFF };
    uint8_t rxData[2] = { 0 };
    uint16_t timeout = 200;

    while (HAL_SPI_GetState(_spi) != HAL_SPI_STATE_READY) {
        if (timeout-- == 0) {
            return 0;
        }
    }

    csEnable();
    HAL_StatusTypeDef status = HAL_SPI_TransmitReceive(_spi, txData, rxData, 2, HAL_MAX_DELAY);
    csDisable();

    if (status != HAL_OK) {
        return 0;
    }

    // 注意顺序：MT6701 发的是高字节在前（Big Endian）
    uint16_t rawData = (rxData[0] << 8) | rxData[1];

    return rawData >> 2; // 高14位有效
}

float MT6701Sensor::getRawAngle() {
    rawdata= getRawData();
    if (rawdata == 0xFFFF) return encoder_angle; // 上一次合法值
    float angle = (static_cast<float>(rawdata) / 16384.0f) * _2PI;
    encoder_angle = angle;
    return angle;
}

void MT6701Sensor::update() {
    float _encoder_angle = getRawAngle(); // 获取当前物理角度

    static bool is_initialized = false;
    if (!is_initialized) {
        _angleLast = _encoder_angle;
        motor_logic_angle = _encoder_angle * _polePairs; // 初始电角度 = 物理角度 × 极对数
        is_initialized = true;
        return;
    }

    // 计算物理角度变化（确保差值在 -π 到 π 之间）
    float diff_physical = cycle_diff(_encoder_angle - _angleLast, _2PI);
    _angleLast = _encoder_angle;

    // 更新电角度（物理角度差 × 极对数）
    // 注意：电角度的周期是 2π，因此归一化周期应为 2π
    float diff_electrical = diff_physical * _polePairs;
    motor_logic_angle = cycle_diff(motor_logic_angle + diff_electrical, _2PI);
}

float MT6701Sensor::getFullAngle() {
    return _fullAngleLast; // 留空，暂不实现
}

float MT6701Sensor::getVelocity() {
    return 0.0f; // 留空，暂不实现
}

float MT6701Sensor::getElectricalAngle() {
    return motor_logic_angle;
}

void MT6701Sensor::initFirstAng(float angle) {
    _angleLast = angle;
    motor_logic_angle = angle;
    _rotationCount = 0;
    _rotationCountLast = 0;
    _fullAngleLast = 0;
}


